/* Module Device Controller*/
#include "MDC.h"
#include "MIRMM.h"

/* Kernel includes. */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"

#include "utilidades.h"

//const AS1_TComData OPEN_HAND = 'O';
//const AS1_TComData CLOSE_HAND = 'C';
#define OPEN_HAND 'O'
#define CLOSE_HAND 'C'
#define RESET_HAND 'R'

static AS1_TComData uiHandState = RESET_HAND;


extern xSemaphoreHandle xNewMotionIntentionSemaphore;
xSemaphoreHandle xNewAngleSemaphore;
static int8_t AnglesNumber = 3;
static int i = 0;
static int16_t iGripShapeMDC; 
static int16_t iApertureRatioMDC;
static int16_t iForceMDC;

static AS1_TComData iProximalThumb[] = { '0', '1' , '2', '3' };
static AS1_TComData iMedialThumb[] = { '0' , '1' , '2' , '3' };
static AS1_TComData iDistalThumb[] = { '0' , '1' , '2' , '3' };


static AS1_TComData iProximalIndexFinger[] = { '0' , '1' , '2' , '3' };
static AS1_TComData iMedialIndexFinger[] = { '0' , '1' , '2' , '3' };
static AS1_TComData iDistalIndexFinger[] = { '0' , '1' , '2' , '3' };

static AS1_TComData iProximalMiddleFinger[] = { '0' , '1' , '2' , '3' };
static AS1_TComData iMedialMiddleFinger[] = { '0' , '1' , '2' , '3' };
static AS1_TComData iDistalMiddleFinger[] = { '0' , '1' , '2' , '3' };

void vTaskCode4( void * pvParameters )
{
   pvParameters = pvParameters;
   char msg[23];
   uint16_t size = 23;
   
   const portTickType xDelay = 1000 / portTICK_RATE_MS;   
   msg = "inicio de tarea 4 -MDC";
   //AS1_SendBlock(msg, sizeof(msg), &size );   
   //AS1_SendChar(0x0A);  /* envia un espacio */         
   vSemaphoreCreateBinary( xNewAngleSemaphore );  
   
   for( ;; )
   {
      xSemaphoreTake( xNewMotionIntentionSemaphore, portMAX_DELAY );
      GetHandGrip( &iGripShapeMDC, &iApertureRatioMDC, &iForceMDC );      
      //HexToScreen( &iApertureRatioMDC ); 
      if(iApertureRatioMDC == 100)
      {
        if(uiHandState == RESET_HAND)
        {
          uiHandState = CLOSE_HAND; 
        }
        else
        {
          uiHandState = OPEN_HAND;
        }
        
      }
      else 
      if(iApertureRatioMDC == 10)
      {
        
        uiHandState = CLOSE_HAND;                
      }
      
      
      xSemaphoreGive( xNewAngleSemaphore );
      msg = "xSemaphoreGive        ";
      //AS1_SendChar(0x0A);  /* envia un espacio */
      //AS1_SendBlock(msg, sizeof(msg), &size );
      //AS1_SendChar(0x0A);  /* send a space */
   }
}

void  MDC_sendAngle(AS1_TComData ch)
{
  AS1_TComData iProximal;
  AS1_TComData iMedial;
  AS1_TComData iDistal;

  switch (ch)
  {
    case 'T':
      {
        MDC_GetThumbAngle( &iProximal, &iMedial, &iDistal);
      }break;
    case 'I':
      {
        MDC_GetIndexFingerAngle( &iProximal, &iMedial, &iDistal );      
      }break;
    case 'M':
      {
        MDC_GetMiddleFingerAngle( &iProximal, &iMedial, &iDistal);
      }break;
  }
  
  AS1_SendChar(ch);
  AS1_SendChar(iProximal);
  AS1_SendChar(iMedial);
  AS1_SendChar(iDistal);
  AS1_SendChar(0x0A);  /* send a space */
}


//trayectorias
void vTaskCode5( void * pvParameters )
{
  int j = 0;
  pvParameters = pvParameters ;
  const portTickType xDelay = 1000 / portTICK_RATE_MS;   
  for(;;)
  {
    j = 0;
    xSemaphoreTake( xNewAngleSemaphore, portMAX_DELAY );
    while(j < AnglesNumber )
    {
      if (uiHandState == OPEN_HAND)
      {
        if(i<AnglesNumber)
          ++i;
      }
      else if(uiHandState == CLOSE_HAND)
      {
        if(i>0)
          --i;
      }
      //MDC_sendAngle('T');
      //MDC_sendAngle('I');
      //MDC_sendAngle('M');
      vTaskDelay( xDelay );    
      ++j;
    }
  }
}


void MDC_GetHandState(AS1_TComData * pState )
{
  *pState = uiHandState;
}

void MDC_ResetHandState()
{
  uiHandState = RESET_HAND;
}

// se sugierer proteger al index "i" con un semaforo
void MDC_GetThumbAngle( AS1_TComData * pProximal, AS1_TComData * pMedial, AS1_TComData * pDistal )
{
  *pProximal = iProximalThumb[i];
  *pMedial = iMedialThumb[i];
  *pDistal = iDistalThumb[i];    
}

void MDC_GetIndexFingerAngle( AS1_TComData * pProximal, AS1_TComData * pMedial, AS1_TComData * pDistal )
{
  *pProximal = iProximalIndexFinger[i];
  *pMedial = iMedialIndexFinger[i];
  *pDistal = iDistalIndexFinger[i];    
}

void MDC_GetMiddleFingerAngle( AS1_TComData * pProximal, AS1_TComData * pMedial, AS1_TComData * pDistal )
{
  *pProximal = iProximalMiddleFinger[i];
  *pMedial = iMedialMiddleFinger[i];
  *pDistal = iDistalIndexFinger[i];    
}

